import numpy as np
import config as cfg

class Sensor():
    def __init__(self, model, data):
        self.model = model
        self.data = data
        
        self.wheel_pos_sensor_names = cfg.WHEEL["sensors"]
        self.imu_sensor_names = cfg.IMU["sensors"]
        self.car_frame_sensor_names = cfg.BASE["sensors"]
        self.laser_sensor = {
            "name": cfg.LASER["name"],
            "count": cfg.LASER["count"],
        }
        
    # 获取速度sensor的数据
    def get_wheel_vel_sensor_data(self):
        vel_sensor_data = []
        for sensor in self.wheel_pos_sensor_names:
            vel_sensor_data.append(float(self.data.sensor(sensor).data[0]))
        return vel_sensor_data
    # 获取imu sensor的数据
    def get_imu_sensor_data(self):
        imu_sensor_data = []
        for sensor in self.imu_sensor_names:
            imu_sensor_data.append(self.data.sensor(sensor).data.tolist())
        return imu_sensor_data
    # 获取car的全局信息
    def get_car_frame_sensor_data(self):
        car_frame_sensor_data = []
        for sensor in self.car_frame_sensor_names:
            car_frame_sensor_data.append(self.data.sensor(sensor).data.tolist())
        return car_frame_sensor_data
    # 获取car的线速度(特权信息)
    def get_car_linear_vel(self):
        car_frame_vel = self.get_car_frame_sensor_data()[1]
        car_linear_vel = pow(pow(car_frame_vel[0], 2) + pow(car_frame_vel[1], 2), 0.5)
        return car_linear_vel
    # 获取激光雷达数据
    def get_laser_data(self):
        laser_data = []
        laser_sensor_name = self.laser_sensor["name"]
        for i in range(self.laser_sensor["count"]):
            laser_data.append(float(self.data.sensor(f"{laser_sensor_name}{i:03d}").data[0]))
        return laser_data